using System;
using System.Collections;
using System.Collections.Generic;

namespace LightCAD.MathLib
{
    public class Sphere
    {
        #region scope properties or methods
        //private static Box3 _box = new Box3();
        //private static Vector3 _v1 = new Vector3();
        //private static Vector3 _v2 = new Vector3();
        private static SphereContext GetContext()
            => ThreadContext.GetCurrThreadContext().SphereCtx;
        #endregion

        #region Properties

        public Vector3 Center;
        public double Radius;

        #endregion

        #region constructor
        public Sphere(Vector3 center = null, double radius = -1)
        {
            if (center == null) center = new Vector3();
            this.Center = center;
            this.Radius = radius;
        }
        #endregion

        #region methods
        public Sphere Set(Vector3 center, double radius)
        {
            this.Center.Copy(center);
            this.Radius = radius;
            return this;
        }
        public Sphere SetFromPoints(Vector3[] points, Vector3 optionalCenter=null)
        {
            Vector3 center = this.Center;
            if (optionalCenter != null)
            {
                center.Copy(optionalCenter);
            }
            else
            {
                GetContext()._box.SetFromPoints(points).GetCenter(center);
            }
            double maxRadiusSq = 0;
            for (int i = 0, il = points.Length; i < il; i++)
            {
                maxRadiusSq = Math.Max(maxRadiusSq, center.DistanceToSquared(points[i]));
            }
            this.Radius = Math.Sqrt(maxRadiusSq);
            return this;
        }
        public Sphere Copy(Sphere sphere)
        {
            this.Center.Copy(sphere.Center);
            this.Radius = sphere.Radius;
            return this;
        }
        public bool IsEmpty()
        {
            return (this.Radius < 0);
        }
        public Sphere MakeEmpty()
        {
            this.Center.Set(0, 0, 0);
            this.Radius = -1;
            return this;
        }
        public bool ContainsPoint(Vector3 point)
        {
            return (point.DistanceToSquared(this.Center) <= (this.Radius * this.Radius));
        }
        public double DistanceToPoint(Vector3 point)
        {
            return (point.DistanceTo(this.Center) - this.Radius);
        }
        public bool IntersectsSphere(Sphere sphere)
        {
            double radiusSum = this.Radius + sphere.Radius;
            return sphere.Center.DistanceToSquared(this.Center) <= (radiusSum * radiusSum);
        }
        public bool IntersectsBox(Box3 box)
        {
            return box.IntersectsSphere(this);
        }
        public bool IntersectsPlane(Plane plane)
        {
            return Math.Abs(plane.DistanceToPoint(this.Center)) <= this.Radius;
        }
        public Vector3 ClampPoint(Vector3 point, Vector3 target)
        {
            double deltaLengthSq = this.Center.DistanceToSquared(point);
            target.Copy(point);
            if (deltaLengthSq > (this.Radius * this.Radius))
            {
                target.Sub(this.Center).Normalize();
                target.MulScalar(this.Radius).Add(this.Center);
            }
            return target;
        }
        public Box3 GetBoundingBox(Box3 target)
        {
            if (this.IsEmpty())
            {
                // Empty sphere produces empty bounding box
                target.MakeEmpty();
                return target;
            }
            target.Set(this.Center, this.Center);
            target.ExpandByScalar(this.Radius);
            return target;
        }
        public Sphere ApplyMatrix4(Matrix4 matrix)
        {
            this.Center.ApplyMatrix4(matrix);
            this.Radius = this.Radius * matrix.GetMaxScaleOnAxis();
            return this;
        }
        public Sphere Translate(Vector3 offset)
        {
            this.Center.Add(offset);
            return this;
        }
        public Sphere ExpandByPoint(Vector3 point)
        {
            if (this.IsEmpty())
            {
                this.Center.Copy(point);
                this.Radius = 0;
                return this;
            }
            var _v1 = GetContext()._v1;
            _v1.SubVectors(point, this.Center);
            double lengthSq = _v1.LengthSq();
            if (lengthSq > (this.Radius * this.Radius))
            {
                // calculate the minimal sphere
                double length = Math.Sqrt(lengthSq);
                double delta = (length - this.Radius) * 0.5;
                this.Center.AddScaledVector(_v1, delta / length);
                this.Radius += delta;
            }
            return this;
        }
        public Sphere Union(Sphere sphere)
        {
            if (sphere.IsEmpty())
            {
                return this;
            }
            if (this.IsEmpty())
            {
                this.Copy(sphere);
                return this;
            }
            if (this.Center.Equals(sphere.Center))
            {
                this.Radius = Math.Max(this.Radius, sphere.Radius);
            }
            else
            {
                var ctx = GetContext();
                var _v1 = ctx._v1;
                var _v2 = ctx._v2;
                _v2.SubVectors(sphere.Center, this.Center).SetLength(sphere.Radius);
                this.ExpandByPoint(_v1.Copy(sphere.Center).Add(_v2));
                this.ExpandByPoint(_v1.Copy(sphere.Center).Sub(_v2));
            }
            return this;
        }
        public bool Equals(Sphere sphere)
        {
            return sphere.Center.Equals(this.Center) && (sphere.Radius == this.Radius);
        }
        public virtual Sphere Clone()
        {
            return new Sphere().Copy(this);
        }
        #endregion

    }
    public sealed class SphereContext
    {
        public readonly Box3 _box = new Box3();
        public readonly Vector3 _v1 = new Vector3();
        public readonly Vector3 _v2 = new Vector3();
    }
}
